namespace
{
void makeWalls(std::vector<Wall> &walls, scalar w, scalar h)
{
        walls.push_back(Wall("0 0 0", w, false));                               // 0
        walls.push_back(Wall("0 0 0", h, true));                                // 1
        walls.push_back(Wall("0 " + std::to_string(h) + " 0", w, false));       // 2
        walls.push_back(Wall(std::to_string(w) + " 0 0", h, true));             // 3
}
}

Environment::Environment(scalar g) :
                _width(1), _height(1), _gravity("0 " + std::to_string(g) + " 0"), _objects(), walls()
{
        makeWalls(walls, _width, _height);
}

Environment::Environment(uint width, uint height, scalar g) :
                _width(width), _height(height), _gravity("0 " + std::to_string(g) + " 0"), _objects(), walls()
{
        makeWalls(walls, _width, _height);
}

Environment::Environment(std::initializer_list<RigidBody*> b, uint width, uint height, scalar g) :
                _width(width), _height(height), _gravity("0 " + std::to_string(g) + " 0"), _objects(b), walls()
{
        makeWalls(walls, _width, _height);
}

Environment::~Environment()
{
        for (auto b : _objects)
                delete b;
}

void Environment::addBody(RigidBody *body)
{
        if (body) _objects.insert(body);
}

void Environment::delBody(RigidBody *body)
{
        if (body) _objects.erase(body);
}

void Environment::correct(RigidBody *b)
{
        ConvexShape *shape = dynamic_cast<ConvexShape*>(b);
        if (shape)
        {
                if ((b->_velocity[0] < 0 && shape->getSupport("-1 0 0")[0] <= 0)
                                || (b->_velocity[0] > 0 && (shape->getSupport("1 0 0")[0] - _width) >= 0)) b->_velocity[0] =
                                -b->_velocity[0] * b->_elastic;

                if ((b->_velocity[1] < 0 && shape->getSupport("0 -1 0")[1] <= 0)
                                || (b->_velocity[1] > 0 && (shape->getSupport("0 1 0")[1] - _height) >= 0)) b->_velocity[1] =
                                -b->_velocity[1] * b->_elastic;
        }
}

void Environment::update(scalar seconds)
{
        auto end = _objects.cend();
        for (scalar step = std::min(.005f, seconds), time = step; time <= seconds; time += step)
        {
                for (RigidBody *b : _objects)
                        if (b->_mass)
                        {
                                b->_velocity += (step * _gravity);
                                b->integrate(step);
//                                correct(b);
                        }

                for (auto i =  _objects.cbegin(); i != end; ++i)
                {
                        for (auto w : walls)
                                ContactSolver::handleCollisionIfAny(**i, w);
                        for (auto j = std::next(i); j != end; ++j)
                                ContactSolver::handleCollisionIfAny(**i, **j);
                }
//				if (ContactSolver::handleCollisionIfAny(**i, **j))
//				{
//					if ((*i)->_mass >= (*j)->_mass) _objects.erase(j);
//					else _objects.erase(i);
//				}
                }
        }

scalar Environment::gravity() const
{
        return _gravity[1];
}

void Environment::gravity(scalar g)
{
        _gravity[1] = g;
}

uint Environment::width() const
{
        return _width;
}

void Environment::width(uint w)
{
        _width = w;
        walls[0].length(w);
        walls[2].length(w);
        walls[3]._position[0] = w;
}

uint Environment::height() const
{
        return _height;
}

void Environment::height(uint h)
{
        _height = h;
        walls[1].length(h);
        walls[2]._position[1] = h;
        walls[3].length(h);
}

void Environment::resize(uint w, uint h)
{
        width(w);
        height(h);
}

const bodySet& Environment::bodies() const
{
        return _objects;
}
